#include <SoftwareSerial.h>
#include <ArduinoJson.h>
#include <Servo.h>

Servo armJoint1;
Servo armJoint2;
Servo armJoint3; 

typedef struct {
    int16_t accelX;
    int16_t accelY;
    int16_t accelZ;
    int16_t gyroX;
    int16_t gyroY;
    int16_t gyroZ;
} SensorData;

//在Arduino UNO板上，初始化串口通信
void setup() {
  Serial.begin(115200);
  servo1.attach(8,500,2000);
  servo2.attach(9,500,2000);
  servo3.attach(10,500,2000);
}

//在loop函数中，检查串口是否有可读取的数据。读取JSON格式的字符串数据，并反序列化为struct_message结构体。
void loop() {
    if (Serial.available() > 0) {
        SensorData data;
        String rawData = Serial.readStringUntil('\n');
        StaticJsonDocument<200> jsonDoc;
        deserializeJson(jsonDoc, rawData);
        data.accelX = jsonDoc["accelX"];
        data.accelY = jsonDoc["accelY"];
        data.accelZ = jsonDoc["accelZ"];
        data.gyroX = jsonDoc["gyroX"];
        data.gyroY = jsonDoc["gyroY"];
        data.gyroZ = jsonDoc["gyroZ"];

//根据转换后的角度值，使用servo.write()函数控制三个舵机转动到相应位置。
void controlServos(SensorData *sensors) {
    float servo1Pos, servo2Pos, servo3Pos;
    servo1Pos = map(sensors->accelX, -17000, 17000, 0, 180);
    servo2Pos = map(sensors->accelY, -17000, 17000, 0, 180);
    servo3Pos = map(sensors->accelZ, -17000, 17000, 0, 180);
    armJoint1.write(servo1Pos);
    armJoint2.write(servo2Pos);
    armJoint3.write(servo3Pos);
    Serial.print(servo1Pos);
    Serial.print(" | ");
    Serial.print(servo2Pos);
    Serial.print(" | ");
    Serial.println(servo3Pos);
    delay(500);
}